#include "common_inc.h"
#include "configurations.h"
#include "st_hardware.h"
#include "mpu6500.h"
#include "eeprom_interface.h"
#include "usart.h"
#include <bsp_tim.h>
#include <bsp_stmflash.h>
#include <bsp_can.h>


/* Component Definitions -----------------------------------------------------*/
BoardConfig_t boardConfig;
Motor motor;
TB67H450 tb67H450;
MT6816 mt6816;
EncoderCalibrator encoderCalibrator(&motor);
Button button1(1, 1000), button2(2, 3000);
void OnButton1Event(Button::Event _event);
void OnButton2Event(Button::Event _event);
Led statusLed;
enum NEED_FIND_HOME_FLAG NeedFindHomeFalg,LastNeedFindHomeFlag;
bool FindedHomeFlag = false,FindHomeFlagUpdataFlag = false,FindHomeNeedReverseFlag = false,UpdatedToFlashFlag = false;
uint32_t FindHomeRealPos;
uint16_t FindHomeNeedBackMs = 0;
uint32_t TofDis = 0;
bool DebugPidvFlag = false;
uint32_t DebugPidvDelMs = 10;

#if  defined(Driver_IMU6) ||  defined(Driver_IRC)
#include "math.h"
#include "bsp_colorLED.h"

IMU6_SIGNAL imu6_data={
        false,
        0,
        0,
        0,
        0,
};


void Read_Electro(void)
{
    if(imu6_data.Find_Home == false ){

            imu6_data.Elector1 = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_6);
            imu6_data.Elector2 = HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_7);
    }
}

float CompareRGB(uint16_t sr,uint16_t sg,uint16_t sb,uint16_t r,uint16_t g,uint16_t b)
{
    float f = float(sr+sb+sg)/(r+b+g);
    r = r*f;
    b = b*f;
    g = g*f;
    return sqrt((sr-r)*(sr-r)+(sg-g)*(sg-g)+(sb-b)*(sb-b));
}

bool GettedGreenColour()
{
    //osDelay(1500);
    if(CompareRGB(345,2448,1107,rgb.r,rgb.g,rgb.b) < 800)
        return true;
    else
        return false;
}

bool GettedYellowColour()
{
    //osDelay(1500);
    if(CompareRGB( 4823, 2666, 1565, rgb.r,rgb.g,rgb.b) < 800)
        return true;
    else
        return false;
}


//void  GetColorData_process(void){
////     if(imu6_data.Start_Led == 1) {
////
////     }
//}
#endif


void motorInit(uint8_t id){
    if(boardConfig.HardVer == DUMMY){
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1300;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }else if(boardConfig.HardVer == MR6){
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 500;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }else if(boardConfig.HardVer == BATTLE_CHARGE_THERE_AXIS){
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2*1000;
                motor.config.motionParams.ratedCurrent = 3.3*1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc = 1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }else if(boardConfig.HardVer == TRANSMIT_PLANE_THERE_AXIS ) {
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 1000;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 100;
                motor.config.ctrlParams.dce.kd = 1000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }
    else if(boardConfig.HardVer == IMU6 ) {
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 800;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 200;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
                motor.config.ctrlParams.dce.kp = 800;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 200;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 800;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 200;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 800;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 200;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 1000;
                motor.config.ctrlParams.dce.ki = 100;
                motor.config.ctrlParams.dce.kd = 800;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 4000;
                motor.config.ctrlParams.dce.ki = 500;
                motor.config.ctrlParams.dce.kd = 2000;
                motor.config.ctrlParams.dce.kv = 5;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }

    else if(boardConfig.HardVer == IRC ) {
        switch (id) {
            case 0x01:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 5400;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 100;
                motor.config.ctrlParams.dce.kv = 3;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x02:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x03:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x04:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x05:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            case 0x06:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
            default:
                motor.config.motionParams.caliCurrent = 2 * 1000;
                motor.config.motionParams.ratedCurrent = 3.3 * 1000;
                motor.config.motionParams.ratedVelocity = 220 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
                motor.config.motionParams.ratedVelocityAcc =
                        1 * 1000 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;     // (mA/s)
                motor.motionPlanner.velocityTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.motionPlanner.positionTracker.SetVelocityAcc(motor.config.motionParams.ratedVelocityAcc);
                motor.config.ctrlParams.dce.kp = 900;
                motor.config.ctrlParams.dce.ki = 20;
                motor.config.ctrlParams.dce.kd = 500;
                motor.config.ctrlParams.dce.kv = 2;
                motor.config.ctrlParams.stallProtectSwitch = false;
                break;
        }
    }


}

void AskTofData(){
    static uint8_t buff[5];
    buff[0] = 0x55;
    buff[1] = 0xAA;
    buff[2] = 0x81;
    buff[3] = 0x00;
    buff[4] = 0xFA;
    HAL_UART_Transmit_DMA(&huart1,(uint8_t *)buff,sizeof(buff));
}

void DataInit(){
    if(boardConfig.HardVer == DUMMY){
        if(HARD_VER_SERIES_NUM == 0 && HARD_VER_ROBOT_NUM == 3 && boardConfig.canNodeId == 6)
            FindedHomeFlag = true;
    }

}

/* Main Entry ----------------------------------------------------------------*/
void Main()
{
    uint64_t serialNum = GetSerialNumber();
    uint16_t defaultNodeID = 0;

    // Change below to fit your situation
    switch (serialNum)
    {
        case 431466563640: //J1
            defaultNodeID = 1;
            break;
        case 384624576568: //J2
            defaultNodeID = 2;
            break;
        case 384290670648: //J3
            defaultNodeID = 3;
            break;
        case 431531051064: //J4
            defaultNodeID = 4;
            break;
        case 431466760248: //J5
            defaultNodeID = 5;
            break;
        case 431484848184: //J6
            defaultNodeID = 6;
            break;
        default:
            break;
    }

  //  boardConfig.configStatus = CONFIG_COMMIT;
    /*---------- Apply EEPROM Settings ----------*/
    // Setting priority is EEPROM > Motor.h
    EEPROM eeprom;
    eeprom.get(0, boardConfig);


    //boardConfig.configStatus = CONFIG_COMMIT;

    if (boardConfig.configStatus != CONFIG_OK) // use default settings
    {
        boardConfig = BoardConfig_t{
                .configStatus = CONFIG_OK,
                .canNodeId = boardConfig.canNodeId,
                .encoderHomeOffset = motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS,
                .defaultMode = Motor::MODE_COMMAND_POSITION,
                .currentLimit = 1 * 1000,    // A
                .velocityLimit = 30 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS, // r/s
                .velocityAcc = 100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS,   // r/s^2
                .calibrationCurrent=2000,
                .dce_kp = 200,
                .dce_kv = 80,
                .dce_ki = 300,
                .dce_kd = 250,
                .enableMotorOnBoot=false,
                .enableStallProtect=false,
                .HardVer = HARD_VER_INIT,
        };
       eeprom.put(0, boardConfig);
    }

    boardConfig.HardVer = HARD_VER_INIT;
    boardConfig.canNodeId = CAN_NODE_ID;

    motorInit(boardConfig.canNodeId);


    /*---------------- Init Motor ----------------*/
    motor.AttachDriver(&tb67H450);
    motor.AttachEncoder(&mt6816);
    motor.controller->Init();
    motor.driver->Init();
    motor.encoder->Init();


    /*------------- Init peripherals -------------*/
    button1.SetOnEventListener(OnButton1Event);
    button2.SetOnEventListener(OnButton2Event);


    /*------- Start Close-Loop Control Tick ------*/
    HAL_Delay(100);
    HAL_TIM_Base_Start_IT(&htim1);  // 100Hz
    HAL_TIM_Base_Start_IT(&htim4);  // 20kHz


//    if (button1.IsPressed() && button2.IsPressed())
//        encoderCalibrator.isTriggered = true;


    printf("\nHG Step Motor\n");
    if(boardConfig.HardVer == DUMMY)
        printf("Robot:DUMMY %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == AR3)
        printf("Robot:AR3 %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == SITO)
        printf("Robot:SITO %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == BATTLE_CHARGE_THERE_AXIS)
        printf("Robot:BATTLE_CHARGE_THERE_AXIS %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == MR6)
        printf("Robot:MR6 %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == TRANSMIT_PLANE_THERE_AXIS)
        printf("Robot:TRANSMIT_PLANE_THERE_AXIS %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == IMU6)
        printf("Robot:IMU6 %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else if(boardConfig.HardVer == IRC)
        printf("Robot:IRC %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    else
        printf("Robot:UNKNOW %d_%d\n",HARD_VER_SERIES_NUM,HARD_VER_ROBOT_NUM);
    printf("SoftVer:%d.%02d\n",SOFT_VER_A,SOFT_VER_B);
    printf("ID:%d\n",boardConfig.canNodeId);

    DataInit();

    for (;;)
    {
        encoderCalibrator.TickMainLoop();


        if (boardConfig.configStatus == CONFIG_COMMIT)
        {

            boardConfig.configStatus = CONFIG_OK;
            eeprom.put(0, boardConfig);
        } else if (boardConfig.configStatus == CONFIG_RESTORE)
        {
            eeprom.put(0, boardConfig);
            HAL_NVIC_SystemReset();
        }

        if(boardConfig.HardVer == MR6 && boardConfig.canNodeId == 6 || boardConfig.HardVer == BATTLE_CHARGE_THERE_AXIS && HARD_VER_ROBOT_NUM == 1 && boardConfig.canNodeId == 3){
            AskTofData();
            HAL_Delay(50);
        }

        if(boardConfig.HardVer == IMU6  || boardConfig.HardVer == IRC){
            //imu6灯光采集
            Read_Electro();
        }

//        AccUpdataAngle();
//    while(count --){
//        tb67H450.SetFocCurrentVector(count,1000);
//    }

        if(DebugPidvFlag){
            motor.controller->SendTarAndCurrPositionToANOTech(DebugPidvDelMs);
        }



    }
}


/* Event Callbacks -----------------------------------------------------------*/
extern "C" void Tim1Callback100Hz()
{
    __HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);

    button1.Tick(10);
    button2.Tick(10);
    statusLed.Tick(10, motor.controller->state);

    if(FindHomeNeedBackMs >= 10)
        FindHomeNeedBackMs -= 10;
    else
        FindHomeNeedBackMs = 0;
}


extern "C" void Tim4Callback20kHz()
{
    __HAL_TIM_CLEAR_IT(&htim4, TIM_IT_UPDATE);

    if (encoderCalibrator.isTriggered)
        encoderCalibrator.Tick20kHz();
    else
        motor.Tick20kHz();
}

bool IsCalibratorKeyOn(){
    if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_6) == GPIO_PIN_RESET)
        return true;
    else
        return false;
}

bool IsCalibratorOtherKeyOn(){

    if(!(boardConfig.HardVer == MR6 && (boardConfig.canNodeId == 1 || boardConfig.canNodeId == 4 || boardConfig.canNodeId == 6))
    && !(boardConfig.HardVer == TRANSMIT_PLANE_THERE_AXIS && boardConfig.canNodeId == 3)){
        return false;
    }

    if(HAL_GPIO_ReadPin(GPIOA,GPIO_PIN_7) == GPIO_PIN_RESET)
        return true;
    else
        return false;
}


void OnButton1Event(Button::Event _event)
{
    switch (_event)
    {
        case ButtonBase::UP:
            break;
        case ButtonBase::DOWN:
            break;
        case ButtonBase::LONG_PRESS:
            HAL_NVIC_SystemReset();
            break;
        case ButtonBase::CLICK:
            if (motor.controller->modeRunning != Motor::MODE_STOP)
            {
                boardConfig.defaultMode = motor.controller->modeRunning;
                motor.controller->requestMode = Motor::MODE_STOP;
            } else
            {
                motor.controller->requestMode = static_cast<Motor::Mode_t>(boardConfig.defaultMode);
            }
            break;
    }
}


void OnButton2Event(Button::Event _event)
{
    switch (_event)
    {
        case ButtonBase::UP:
            break;
        case ButtonBase::DOWN:
            break;
        case ButtonBase::LONG_PRESS:
            switch (motor.controller->modeRunning)
            {
                case Motor::MODE_COMMAND_CURRENT:
                case Motor::MODE_PWM_CURRENT:
                    motor.controller->SetCurrentSetPoint(0);
                    break;
                case Motor::MODE_COMMAND_VELOCITY:
                case Motor::MODE_PWM_VELOCITY:
                    motor.controller->SetVelocitySetPoint(0);
                    break;
                case Motor::MODE_COMMAND_POSITION:
                case Motor::MODE_PWM_POSITION:
                    motor.controller->SetPositionSetPoint(0);
                    break;
                case Motor::MODE_COMMAND_Trajectory:
                case Motor::MODE_STEP_DIR:
                case Motor::MODE_STOP:
                    break;
            }
            break;
        case ButtonBase::CLICK:
            motor.controller->ClearStallFlag();
            break;
    }
}


void AplyHomeWithPhysicLimit(){
    FindHomeRealPos = motor.encoder->angleData.rectifiedAngle;
    if(FindHomeFlagUpdataFlag || boardConfig.encoderHomeOffset == motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS)
    {
        motor.controller->ApplyPosAsHomeOffset();
        boardConfig.encoderHomeOffset = motor.config.motionParams.encoderHomeOffset %
                                        motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
        boardConfig.configStatus = CONFIG_COMMIT;
    }else
        motor.controller->ApplyPosNearHomeOffset();


    if (motor.controller->modeRunning != Motor::MODE_COMMAND_POSITION)
    {
        motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
        motor.controller->SetCtrlMode(Motor::MODE_COMMAND_POSITION);
    }
    motor.config.motionParams.ratedVelocity =
            (int32_t) (10 * (float) motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS);
    motor.controller->SetPositionSetPoint(0);

    //motor.controller->SetVelocitySetPoint(0);

    //if(boardConfig.canNodeId != 3)
    //    motor.controller->SetCtrlMode(Motor::MODE_STOP);

    motorInit(boardConfig.canNodeId);
    NeedFindHomeFalg = IS_NOT_NEED;
    FindedHomeFlag = true;
    printf("find home success\n");

};

bool IsSynMotor(uint8_t id){

    if(boardConfig.HardVer == TRANSMIT_PLANE_THERE_AXIS){
        if(boardConfig.canNodeId == 5 && id == 2 || boardConfig.canNodeId == 2 && id == 5)
            return true;
        else if(boardConfig.canNodeId == 3 && id == 6 || boardConfig.canNodeId == 6 && id == 3)
            return true;
    }else if(boardConfig.HardVer == BATTLE_CHARGE_THERE_AXIS){
        if(HARD_VER_ROBOT_NUM == 1 && boardConfig.canNodeId == 1 && id == 4 || boardConfig.canNodeId == 4 && id == 1)
            return true;
    }
    return false;
}